#include <vector>
#include <Eigen/QR>
#include "helpers.h"
#include "matplotlibcpp.h"
#include "MPC.h"

namespace plt = matplotlibcpp;

using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::vector;

int main() 
{
  MPC mpc;
  int iters = 60; // 设置迭代次数: 60次

  // 构造一条参考路径
  VectorXd ptsx(2), ptsy(2); // 设置2个点, 即为1条直线
  ptsx << -100, 100;
  ptsy << -1, -1;
  auto coeffs = polyfit(ptsx, ptsy, 1); // 参考路径: 参考路径为直线

  // 设置车辆初始状态
  double x = -1;  // x坐标
  double y = 10;  // y坐标
  double psi = 0; // 航向
  double v = 10;  // 速度
  double cte = polyeval(coeffs, x) - y; // 初始横向跟踪误差: 参考点 - 车辆y轴
  // 方向误差: 由于符合是从0开始，方向误差是 -f'(x)
  // 系数得导数coeffs[0] + coeffs[1]*x --> coeffs[1]
  double epsi = psi - atan(coeffs[1]); // 初始航向误差: 当前航向 - 对应参考点的切线角

  // 存储车辆的初始状态量
  VectorXd state(6); 
  state << x, y, psi, v, cte, epsi; // 设置车辆原始状态量

  // 用向量存储每一次迭代的结果，用于绘制图形
  vector<double> x_vals = {state[0]};   // x坐标
  vector<double> y_vals = {state[1]};   // y坐标
  vector<double> psi_vals = {state[2]}; // 航向
  vector<double> v_vals = {state[3]};   // 速度
  vector<double> cte_vals = {state[4]}; // 横向误差
  vector<double> epsi_vals = {state[5]};// 航向误差
  // 求解量
  vector<double> delta_vals = {}; // 转角
  vector<double> a_vals = {}; // 加速度(油门)

  for (int i = 0; i < iters; ++i) {
    cout << "Iteration " << i << endl;

    //给定初始状态求解模型，返回下一个状态和执行动作(以向量的形式返回)
    // vars[0,...,5]: 车辆状态
    // vars[6,7]: 控制量
    auto vars = mpc.Solve(state, coeffs);

    // 更新车辆状态
    state << vars[0], vars[1], vars[2], vars[3], vars[4], vars[5];

    // 打印车辆状态和控制量
    cout << "x = " << vars[0] << endl;
    cout << "y = " << vars[1] << endl;
    cout << "psi = " << vars[2] << endl;
    cout << "v = " << vars[3] << endl;
    cout << "cte = " << vars[4] << endl;
    cout << "epsi = " << vars[5] << endl;
    cout << "delta = " << vars[6] << endl;
    cout << "a = " << vars[7] << endl;
    cout << endl;

    //保存下一时刻状态量
    x_vals.push_back(vars[0]);      // x坐标
    y_vals.push_back(vars[1]);      // y坐标
    psi_vals.push_back(vars[2]);    // 航行角
    v_vals.push_back(vars[3]);      // 速度
    cte_vals.push_back(vars[4]);    // cte
    epsi_vals.push_back(vars[5]);   // epsi
    // 保存控制量
    delta_vals.push_back(vars[6]);  // delta
    a_vals.push_back(vars[7]);      // a_vals
  }

  // 绘制数据: 可视化结果
  //plt::subplot(3, 1, 1);
  //plt::figure();
  //plt::title("CTE");
  //plt::plot(cte_vals);
  
  //plt::subplot(3, 1, 2);
  //plt::figure();
  //plt::title("Delta (Radians)");
  //plt::plot(delta_vals);
  
  //plt::subplot(3, 1, 3);
  //plt::figure();
  //plt::title("Velocity");
  //plt::plot(v_vals);


  /**预测的位置信息**/
  //plt::subplot(4,2,1);
  plt::figure();
  plt::title("x/y of prediction");
  plt::plot(x_vals, y_vals);
  plt::xlabel("x");
  plt::ylabel("y");
 
  //航向信息
  //plt::subplot(4,2,2);
  plt::figure();
  plt::title("psi prediction");
  plt::plot(psi_vals);
  plt::ylabel("psi");

  //速度信息
  //plt::subplot(4,2,3);
  plt::figure();
  plt::title("v prediction");
  plt::plot(v_vals);
  plt::ylabel("v");

  //cte
  //plt::subplot(4,2,4);
  plt::figure();
  plt::title("cte error prediction");
  plt::plot(cte_vals);
  plt::ylabel("cte");

  //epsi
  //plt::subplot(4,2,5);
  plt::figure();
  plt::title("epsi prediction");
  plt::plot(epsi_vals);
  plt::ylabel("epsi");

  //delta(控制信息)
  //plt::subplot(4,2,6);
  plt::figure();
  plt::title("delta control of prediction");
  plt::plot(delta_vals);
  plt::ylabel("delta");

  //a(控制信息)
  //plt::subplot(4,2,7);
  plt::figure();
  plt::title("a control of prediction");
  plt::plot(a_vals);
  plt::ylabel("a");

  plt::show();
  
  return 0;
}